/*=========================================================================

  Author: Christine Xu

=========================================================================*/
#ifndef _namicSphericalHarmonicPolynomial_txx
#define _namicSphericalHarmonicPolynomial_txx

#include "SphericalHarmonicPolynomial.h"

#include <math.h>
#include <iostream>

namespace neurolib
{
#ifndef M_PI 
#define M_PI 3.1415926535897932
#endif
#ifndef M_PI_2
#define M_PI_2 1.5707963267948966
#endif

template< unsigned int TDimension >
SphericalHarmonicPolynomial< TDimension >::SphericalHarmonicPolynomial()
{
  //m_Dimension = 2;
}

template< unsigned int TDimension >
SphericalHarmonicPolynomial< TDimension >::~SphericalHarmonicPolynomial()
{
  
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::SetCoefs(SphericalHarmonicPolynomial< TDimension >::CoefListType& coeflist)
{
  if(coeflist.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The input coefficient list is empty!");
  unsigned int d = coeflist[0].GetPointDimension();
  if(!(TDimension == d))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The dimension of the input coefficient list does not match the specified dimension!");
  m_Coefs = coeflist;
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::Evaluate(unsigned int from_l,
                                    unsigned int to_l, double phi, double theta,
                                    double* sum)
{
  //if(m_Dimension != 2)
  //  throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Only Spherical Harmonics of dimension two is currently implemented.");
  //if(size > TDimension)
  //   throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Dimension of the output sum exceeds the dimension of the coefficients.");
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(from_l > to_l)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The starting degree should be smaller or equal to the ending degree.");
  if(to_l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evalueated degree mustn't exceed the size of the coefficients.");
  if(m_Coefs.size() < (to_l+1)*(to_l+1))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients size mismatch.");
  
  unsigned int l, m, coord;
  
  for (coord= 0; coord < TDimension; coord++) 
  {
     sum[coord]= 0.0;
     
     for (l= from_l; l <= to_l; l++) //for the from_l degree to to_l degree
     {
         double* plm = new double[l+1];
         for (m= 0; m <= l; m++)
         {
           double coef = sqrt((2*l+1.0)/4.0/M_PI / fac_quot(l+m, l-m));
           double row = plgndr_row(l, m, cos(theta));
           plm[m] = coef * row;
         }
         
         sum[coord]+= m_Coefs[l*l][coord] * plm[0];
         
        for (m= 1; m <= l; m++)
        {
          double sin_t = sin(m*phi);
          double cos_t = cos(m*phi);
          sum[coord]+= ( cos_t * m_Coefs[l*l+2*m-1][coord]//real part
            +sin_t * m_Coefs[l*l+2*m ][coord] ) //imaginary part
            * plm[m];
        }
        delete []plm;
     }
  }
}


/*
output:
p_ptr:  the value of specified list of Associated Legendre polynomials (vertically, columnly)

input:
m:      the order of the polynomials
l:      the maximal degree of the polynomials
x:      the input parameter of the Associated Legendre polynomials

Legendre Polynomials come from the Sturm-Liouville Boundary Value Problem:
(1-x^2)y'' - 2xy' + n(n+1)y = 0

The Legendre Polynomials have the recurrence relation of:
(n+1)P_n+1(x) = (2n+1)xP_n(x) - n P_n-1(x)

The associate Legendre Polynomials are defined using Legendre Polynomials,
and they also obey the following recurrence relations
(l-m)P^m_l(x) = x(2l-1)P^m_l-1(x) - (l+m-1)P^m_l-2(x)

More about Legendre Polynomials:
http://mathworld.wolfram.com/LegendrePolynomial.html
*/
template< unsigned int TDimension >
double SphericalHarmonicPolynomial< TDimension >::plgndr_row(int l, int m, double x)
{
   double fact,pll,pmm,pmmp1,somx2;
   int i,ll;
   
   pll = 0;
   if (m < 0 || m > l || fabs(x) > 1.0)
   {
      std::cout<<"Bad arguments in routine PLGNDR"<<std::endl;
      exit(1);
   }
   pmm=1.0;
   if (m > 0) 
   {
      somx2=sqrt((1.0-x)*(1.0+x));
      fact=1.0;
      for (i=1;i<=m;i++) 
      {
         pmm *= -fact*somx2;
         fact += 2.0;
      }
   }
   //*p_ptr++= pmm;
   if (l == m)
      return pmm;
   else 
   {
      pmmp1=x*(2*m+1)*pmm;
      //*p_ptr++= pmmp1;
      if (l == (m+1))
         return pmmp1;
      else 
      {
         for (ll=(m+2);ll<=l;ll++) 
         {
          pll=(x*(2*ll-1)*pmmp1-(ll+m-1)*pmm)/(ll-m);
          //*p_ptr++= pll;
          pmm=pmmp1;
          pmmp1=pll;
         }
         return pll;
       }
   }
}

/* factorial quotient a!/b!*/
template< unsigned int TDimension >
double SphericalHarmonicPolynomial< TDimension >::fac_quot(int a, int b)
{
   double res= 1;
   while (a > b) res*= a--;
   return res;
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateAt(unsigned int l, unsigned int m, double phi, double theta, double* result)
{
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evalueated degree mustn't exceed the size of the coefficients.");
  
  if ( l < m )
  {
    result[0] = result[1] = 0 ;
    return ;
  }

  double coef = sqrt((2*l+1.0)/4.0/M_PI / fac_quot(l+m, l-m));
  double plm = plgndr_row(l, m, cos(theta));
  double factor = coef * plm ;

  double sin_t = sin(m*phi);
  double cos_t = cos(m*phi);
  
  result[0] = cos_t * factor ;
  result[1] = sin_t * factor ;
}


template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateDTheta(unsigned int from_l,
                                    unsigned int to_l, double phi, double theta,
                                    double* sum)
{
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(from_l > to_l)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The starting degree should be smaller or equal to the ending degree.");
  if(to_l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evaluated degree mustn't exceed the size of the coefficients.");
  if(m_Coefs.size() < (to_l+1)*(to_l+1))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients size mismatch.");
  
  int l, m, coord;
  
  double cotTheta = cos(theta)/sin(theta) ;
  double cosPhi = cos (phi) ;
  double sinPhi = sin (phi) ;

  double realPart, imPart, temp ;
  double eval0[2], eval1[2], coef[2] ;

  for (coord= 0 ; coord < TDimension; coord++) 
  {
    sum[coord] = 0. ;
  }

  for (l= from_l; l <= to_l; l++) //for the from_l degree to to_l degree
  {
    for (m= 0; m <= l; m++)
    {
      this->EvaluateAt (l, m, phi, theta, eval0) ;
      this->EvaluateAt (l, m+1, phi, theta, eval1) ;

      temp = sqrt (l + l*l - m - m*m) ;

      for (coord= 0; coord < TDimension; coord++) 
      {
        this->GetCoefAt(l, m, coord, coef) ;
        realPart = m * cotTheta * eval0[0] + temp * (cosPhi * eval1[0] + sinPhi * eval1[1]) ;
        imPart =   m * cotTheta * eval0[1] + temp * (cosPhi * eval1[1] - sinPhi * eval1[0]) ;

        sum[coord] += realPart * coef[0] + imPart * coef[1] ;
      }
    }
  }
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateDPhi(unsigned int from_l,
                                    unsigned int to_l, double phi, double theta,
                                    double* sum)
{
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(from_l > to_l)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The starting degree should be smaller or equal to the ending degree.");
  if(to_l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evaluated degree mustn't exceed the size of the coefficients.");
  if(m_Coefs.size() < (to_l+1)*(to_l+1))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients size mismatch.");
  
  int l, m, coord;
  
  double eval[2] ;
  double coefs[2] ;
  double realPart ;
  double imPart;

  for (coord= 0 ; coord < TDimension; coord++) 
  {
    sum[coord] = 0. ;
  }
      
  for (l= from_l; l <= to_l; l++) //for the from_l degree to to_l degree
  {
    for (m= 0; m <= l; m++)
    {
      this->EvaluateAt(l, m, phi, theta, eval) ;

      for (coord= 0 ; coord < TDimension; coord++) 
      {
        this->GetCoefAt(l, m, coord, coefs) ;
        realPart = - eval[1] ;
        imPart = eval[0] ;
    
        sum[coord] += ( realPart * coefs[0] + imPart * coefs[1] ) * m ;
      }
    }
  }
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateDDThetaTheta(unsigned int from_l,
                                    unsigned int to_l, double phi, double theta,
                                    double* sum)
{
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(from_l > to_l)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The starting degree should be smaller or equal to the ending degree.");
  if(to_l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evaluated degree mustn't exceed the size of the coefficients.");
  if(m_Coefs.size() < (to_l+1)*(to_l+1))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients size mismatch.");
  
  double cotTheta = 1. / tan (theta) ;
  double cotTheta2 = cotTheta * cotTheta ;
  double cscTheta = 1. / sin (theta);
  double cscTheta2 = cscTheta * cscTheta ;
  double cosPhi = cos (phi) ;
  double sinPhi = sin (phi) ;
  double cos2Phi = cos (2*phi) ;
  double sin2Phi = sin (2*phi) ;

  int l, m, coord;
  for (coord= 0 ; coord < TDimension; coord++) 
  {
    sum[coord] = 0. ;
  }
  
  double eval0[2], eval1[2], eval2[2] ;
  double coefs[2] ;
  double realPart, imPart;
  double factor0, factor1, factor2 ;
  double temp1, temp2 ;
      
  for (l= from_l; l <= to_l; l++) //for the from_l degree to to_l degree
  {
    for (m= 0; m <= l; m++)
    {
      this->EvaluateAt(l, m,   phi, theta, eval0) ;
      this->EvaluateAt(l, m+1, phi, theta, eval1) ;
      this->EvaluateAt(l, m+2, phi, theta, eval2) ;
      factor0 = sqrt (l + l*l - m*(m+1)) ;    
      factor1 = l + l*l - (m+1)*(m+2) ;    
      if ( factor1 > 0 )
      {
        factor2 = factor0 * sqrt(factor1) ;
      }
      else
      {
        factor2 = 0 ;
      }      
      temp1 = m * (-cscTheta2 + m*cotTheta2) ;
      temp2 = cotTheta * factor0 * (2*m + 1) ;
  
      for (coord= 0 ; coord < TDimension; coord++) 
      {
        this->GetCoefAt(l, m, coord, coefs) ;
      
        realPart = eval0[0] * temp1 + 
                  (eval1[0]*cosPhi + eval1[1]*sinPhi) * temp2 +  
                  (eval2[0]*cos2Phi + eval2[1]*sin2Phi) * factor2 ;


        imPart = eval0[1] * temp1 + 
                (eval1[1]*cosPhi - eval1[0]*sinPhi) * temp2 + 
                (eval2[1]*cos2Phi - eval2[0]*sin2Phi) * factor2 ;
              
  
        sum[coord] += ( realPart * coefs[0] + imPart * coefs[1] ) ;
      }
    
    }
  }

}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateDDThetaPhi(unsigned int from_l,
                                    unsigned int to_l, double phi, double theta,
                                    double* sum)
{
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(from_l > to_l)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The starting degree should be smaller or equal to the ending degree.");
  if(to_l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evaluated degree mustn't exceed the size of the coefficients.");
  if(m_Coefs.size() < (to_l+1)*(to_l+1))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients size mismatch.");
  
  int l, m, coord;
  
  double eval0[2], eval1[2] ;
  double coefs[2] ;
  double realPart ;
  double imPart;
  double factor ;

  double cotTheta = 1. / tan (theta) ;
  double cosPhi = cos (phi) ;
  double sinPhi = sin (phi) ;

  for (coord= 0 ; coord < TDimension; coord++) 
  {
    sum[coord] = 0. ;
  }
      
  for (l= from_l; l <= to_l; l++) //for the from_l degree to to_l degree
  {
    for (m= 0; m <= l; m++)
    {
      this->EvaluateAt(l, m, phi, theta, eval0) ;
      this->EvaluateAt(l, m+1, phi, theta, eval1) ;
      factor = sqrt (l + l*l - m - m*m) ;    

      for (coord= 0 ; coord < TDimension; coord++) 
      {
        this->GetCoefAt(l, m, coord, coefs) ;
        realPart = m * cotTheta * eval0[0] + factor * (cosPhi*eval1[0] + sinPhi*eval1[1]) ;
        imPart = -m * cotTheta * eval0[1] + factor * (sinPhi*eval1[0] - cosPhi*eval1[1]) ;
    
        sum[coord] += ( realPart * coefs[0] + imPart * coefs[1] ) * m ;
      }
    }
  }

}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateDDPhiPhi(unsigned int from_l,
                                    unsigned int to_l, double phi, double theta,
                                    double* sum)
{
  if(m_Coefs.empty())
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients must be specified.");
  if(from_l > to_l)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The starting degree should be smaller or equal to the ending degree.");
  if(to_l > m_Degree)
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "The evaluated degree mustn't exceed the size of the coefficients.");
  if(m_Coefs.size() < (to_l+1)*(to_l+1))
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients size mismatch.");
  
  int l, m, coord;
  
  double eval[2] ;
  double coefs[2] ;
  double realPart ;
  double imPart;

  for (coord= 0 ; coord < TDimension; coord++) 
  {
    sum[coord] = 0. ;
  }
      
  for (l= from_l; l <= to_l; l++) //for the from_l degree to to_l degree
  {
    for (m= 0; m <= l; m++)
    {
      this->EvaluateAt(l, m, phi, theta, eval) ;

      for (coord= 0 ; coord < TDimension; coord++) 
      {
        this->GetCoefAt(l, m, coord, coefs) ;
        realPart = eval[0] ;
        imPart = eval[1] ;
    
        sum[coord] -= ( realPart * coefs[0] + imPart * coefs[1] ) * m * m ;
      }
    }
  }
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::GetCoefAt (unsigned int l, unsigned int m, unsigned int coord, double *coef)
{
  if ( m == 0 )
  {
    coef[0] = m_Coefs[l*l][coord] ;
    coef[1] = 0 ;
  }
  else
  {
    coef[0] = m_Coefs[l*l+2*m-1][coord] ;
    coef[1] = m_Coefs[l*l+2*m  ][coord] ;
  }
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::GetUnitNormal(unsigned int from_l, unsigned int to_l, double phi, double theta, double *n)
{
  double dtheta[3], dphi[3] ;
  double length ;

  this->EvaluateDTheta(from_l, to_l, phi, theta, dtheta) ;
  this->EvaluateDPhi(from_l, to_l, phi, theta, dphi) ;

  cross (dtheta, dphi, n) ;

  length = sqrt (n[0] * n[0] + n[1] * n[1] + n[2] * n[2]) ;
  if ( length != 0 )
  {
    length = 1./length ;
    n[0] *= length ;
    n[1] *= length ;
    n[2] *= length ;
  }
}


template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateFirstFundamentalForm (unsigned int from_l, unsigned int to_l, double phi, double theta, double *result) 
{
  double dtheta[3], dphi[3] ;

  this->EvaluateDTheta(from_l, to_l, phi, theta, dtheta) ;
  this->EvaluateDPhi(from_l, to_l, phi, theta, dphi) ;

  result[0] = dot (dtheta, dtheta) ;
  result[1] = dot (dtheta, dphi) ;
  result[2] = dot (dphi, dphi) ;
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::EvaluateSecondFundamentalForm (unsigned int from_l, unsigned int to_l, double phi, double theta, double *result) 
{
  double dThetaTheta[3], dThetaPhi[3], dPhiPhi[3], normal[3] ;
  
  this->EvaluateDDThetaTheta (from_l, to_l, phi, theta, dThetaTheta) ;
  this->EvaluateDDThetaPhi   (from_l, to_l, phi, theta, dThetaPhi) ;
  this->EvaluateDDPhiPhi     (from_l, to_l, phi, theta, dPhiPhi) ;
  this->GetUnitNormal        (from_l, to_l, phi, theta, normal) ;

  result[0] = dot (dThetaTheta, normal) ;
  result[1] = dot (dThetaPhi, normal) ;
  result[2] = dot (dPhiPhi, normal) ;
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::GetPrincipalCurvatures (unsigned int from_l, unsigned int to_l, double phi, double theta, double *kappa) 
{
  double first[3], second[3] ;
  double a, b, c, delta, sqrtDelta ;
  double lambda1, lambda2 ;

  this->EvaluateFirstFundamentalForm (from_l, to_l, phi, theta, first) ;
  this->EvaluateSecondFundamentalForm (from_l, to_l, phi, theta, second) ;

  // solve for lambda1, lambda2 
  a = first[1]*second[2] - first[2]*second[1] ;
  b = first[0]*second[2] - first[2]*second[0] ;
  c = first[0]*second[1] - first[1]*second[0] ;

  if ( a == 0 ) 
  {
    if ( b != 0 )
    {
      lambda1 = lambda2 = -c / b ;
    }
    else
    {
      lambda1 = lambda2 = 0 ;
    }
  }
  else
  {
    delta = b * b - 4 * a * c ;
    if ( delta < 0 ) 
    {
      throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Complex roots to lambda.");
    }

    sqrtDelta = sqrt(delta) ;
    lambda1 = (-b - sqrtDelta) / (2 * a) ;
    lambda2 = (-b + sqrtDelta) / (2 * a) ;
  }

  // compute the principal curvatures
  double quotient ;

  quotient = first[0] + 2*first[1]*lambda1 + first[2]*lambda1*lambda1 ;
  if ( quotient != 0 ) 
    kappa[0] = (second[0] + 2*second[1]*lambda1 + second[2]*lambda1*lambda1) / quotient ;
  else
    kappa[0] = 0 ;

  quotient = first[0] + 2*first[1]*lambda2 + first[2]*lambda2*lambda2 ;
  if ( quotient != 0 )
    kappa[1] = (second[0] + 2*second[1]*lambda2 + second[2]*lambda2*lambda2) / quotient ;
  else 
    kappa[1] = 0 ;
  

  /*this->EvaluateSecondFundamentalForm (from_l, to_l, phi, theta, second) ;
  b = -second[0] - second[2] ;
  c = (second[0] * second[2]) - second[1]*second[1] ;
  delta = b * b - 4 * c + 0.0000001 ;
  if ( delta < 0 ) 
  {
      throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Complex roots to lambda.");
  }
  sqrtDelta = sqrt ( delta ) ;
  kappa[0] = (-b - sqrtDelta) / 2 ;
  kappa[1] = (-b + sqrtDelta) / 2 ;
  */
  // make sure kappa0 is larger than kappa1
  if ( kappa[0] < kappa[1] )
  {
    double temp = kappa[0] ;
    kappa[0] = kappa[1] ;
    kappa[1] = temp ;
  }
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::GetPrincipalDirectionsUV (unsigned int from_l, unsigned int to_l, double phi, double theta, double *uv) 
{
  double first[3], second[3] ;
  this->EvaluateFirstFundamentalForm (from_l, to_l, phi, theta, first) ;
  this->EvaluateSecondFundamentalForm (from_l, to_l, phi, theta, second) ;
  
  // compute the matrix A = M_I^-1 * M_II = ( (p q) (r s) ) 
  double p, q, r, s ;
  p = first[2]*second[0] - first[1]*second[1] ;
  q = first[2]*second[1] - first[1]*second[2] ;
  r = first[0]*second[1] - first[1]*second[0] ;
  s = first[0]*second[2] - first[1]*second[1] ;

  // the characteristic equation of the matrix A is lambda2 + b lambda + c = 0 (a=1)
  double b, c, sqrtdelta, lambda[2] ;
  b = -p - s ;
  c = p * s - q * r ;
  sqrtdelta = sqrt ( b * b - 4 * c ) ;
  lambda[0] = ( -b + sqrtdelta ) / 2 ;
  lambda[1] = ( -b - sqrtdelta ) / 2 ;

  // then, the eigenvalues of A, which are the principal directions in the parameter space, are:
  int i ;
  for ( i = 0 ; i < 2 ; i++ )
  {
    uv[2*i] = 1 ;
    if ( q != 0 )
    {
      uv[2*i+1] = ( -p + lambda[i] ) / q ;
    }
    else if ( lambda[i] - s != 0 )
    {
      uv[2*i+1] = r / (lambda[i] - s) ;
    }
    else
    {
      uv[2*i] = uv[2*i+1] = 0 ;
    }
    double length = sqrt ( uv[2*i+1] * uv[2*i+1] + 1 ) ;
    uv[2*i] /= length ;
    uv[2*i+1] /= length ;
  }  
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::GetPrincipalDirections (unsigned int from_l, unsigned int to_l, double phi, double theta, double *dir) 
{
  double uv[4] ;
  this->GetPrincipalDirectionsUV (from_l, to_l, phi, theta, uv) ;

  // pull back to the xyz space
  double dtheta[3], dphi[3] ;
  this->EvaluateDTheta (from_l, to_l, phi, theta, dtheta) ;
  this->EvaluateDPhi (from_l, to_l, phi, theta, dphi) ;

  dir[0] = uv[0] * dtheta[0] + uv[1] * dphi[0] ;
  dir[1] = uv[0] * dtheta[1] + uv[1] * dphi[1] ;
  dir[2] = uv[0] * dtheta[2] + uv[1] * dphi[2] ;

  dir[3] = uv[2] * dtheta[0] + uv[3] * dphi[0] ;
  dir[4] = uv[2] * dtheta[1] + uv[3] * dphi[1] ;
  dir[5] = uv[2] * dtheta[2] + uv[3] * dphi[2] ;

  // finally, make them unit vectors
  double l = sqrt (dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]) ;  
  if ( l != 0 )
  {
    l = 1./l ;
    dir[0] *= l ;
    dir[1] *= l ;
    dir[2] *= l ;
  }
  
  l = sqrt (dir[3] * dir[3] + dir[4] * dir[4] + dir[5] * dir[5]) ;
  if ( l != 0 )
  {
    l = 1./l ;
    dir[3] *= l ;
    dir[4] *= l ;
    dir[5] *= l ;
  }
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::ComputePrincipalCurve (unsigned int from_l, unsigned int to_l, double startPhi, double startTheta, double *curve, double stepSize, int curveLength) 
{
  int i, curveIndex ;
  double x[3] ;
  double phi, theta ;
  double dir[4], prevDir[2] ;
  double orientation ;

  phi = startPhi ;
  theta = startTheta ;

  prevDir[0] = prevDir[1] = 1 ;

  for ( i = curveIndex = 0 ; i < curveLength ; i++, curveIndex += 3 )
  {
    this->Evaluate(from_l, to_l, phi, theta, x) ;    
    this->GetPrincipalDirectionsUV (from_l, to_l, phi, theta, dir) ;
    if ( dir[0] * prevDir[0] + dir[1] * prevDir[1] < 0 )
    {
      dir[0] *= -1 ;
      dir[1] *= -1 ;
    }
    phi += dir[1] * stepSize ;
    theta += dir[0] * stepSize ;
    curve[curveIndex] = x[0] ;
    curve[curveIndex+1] = x[1] ;
    curve[curveIndex+2] = x[2] ;
    prevDir[0] = dir[0] ;
    prevDir[1] = dir[1] ;
  }
}

template< unsigned int TDimension >
double SphericalHarmonicPolynomial< TDimension >::ComputeC (unsigned int from_l, unsigned int to_l, double phi, double theta) 
{
  double kappa[2] ;
  double r ;
  double c ;

  this->GetPrincipalCurvatures (from_l, to_l, phi, theta, kappa) ;
  r = sqrt ( (kappa[0] * kappa[0] + kappa[1] * kappa[1]) / 2 ) ;  
  if ( r > 0 )
  {
    c = 2 * log (r) / M_PI ;
  }
  else // -"infinity"
  {
    c = -20 ;
  }
   
  return c ;
}

template< unsigned int TDimension >
double SphericalHarmonicPolynomial< TDimension >::ComputeS (unsigned int from_l, unsigned int to_l, double phi, double theta) 
{
  double kappa[2] ;
  double s ;

  this->GetPrincipalCurvatures (from_l, to_l, phi, theta, kappa) ;
  if ( kappa[0] != kappa[1] )
  {
	s = 2 * atan ( ( kappa[0] + kappa[1] ) / ( kappa[1] - kappa[0] ) ) / M_PI ;
  }
  else
  {
	if ( kappa[0] > 0 )
	{
	  s = 1;
	}
	else
	{
	  s = -1;
	}
  }

  return s ;
}

template< unsigned int TDimension >
void SphericalHarmonicPolynomial< TDimension >::cross ( double *u, double *v, double *result ) 
{
  result[0] = u[1]*v[2] - u[2]*v[1] ;
  result[1] = u[2]*v[0] - u[0]*v[2] ;
  result[2] = u[0]*v[1] - u[1]*v[0] ;
}

template< unsigned int TDimension >
double SphericalHarmonicPolynomial< TDimension >::dot ( double *u, double *v ) 
{
  double result ;

  result = u[0]*v[0] + u[1]*v[1] + u[2]*v[2] ;

  return result ;
}

}//end namespace neurolib

#endif
